import matplotlib.pyplot as plt
import numpy as np 
import math

# 坐标旋转
def Nrotate(angle,valuex,valuey,pointx,pointy):
    '''
    绕pointx,pointy逆时针旋转
    (角度，x坐标集，y坐标集，圆心x，圆心y)
    '''
    angle = math.radians(angle) #角度
    valuex = np.array(valuex)
    valuey = np.array(valuey)
    nRotatex = (valuex-pointx)*math.cos(angle) - (valuey-pointy)*math.sin(angle) + pointx
    nRotatey = (valuex-pointx)*math.sin(angle) + (valuey-pointy)*math.cos(angle) + pointy
    
    return nRotatex, nRotatey

def Srotate(angle,valuex,valuey,pointx,pointy):
    '''
    绕pointx,pointy顺时针旋转
    (角度，x坐标集，y坐标集，圆心x，圆心y)
    '''
    angle = math.radians(angle) #角度
    valuex = np.array(valuex)
    valuey = np.array(valuey)
    sRotatex = (valuex-pointx)*math.cos(angle) + (valuey-pointy)*math.sin(angle) + pointx
    sRotatey = (valuey-pointy)*math.cos(angle) - (valuex-pointx)*math.sin(angle) + pointy
    
    return sRotatex,sRotatey

def Angle(x_value,y_value):
    '''   
    夹角计算：
    直线与X轴的夹角，坐标：A(0,0) B(2,4)
    x_value = [0,2] #x坐标集
    y_value = [0,4] #y坐标集 
    '''
    dx1 = x_value[1] - x_value[0]
    dy1 = y_value[1] - y_value[0]
    angle1 = math.atan2(dy1, dx1)
    angle1 = int(angle1 * 180/math.pi)
    
    return angle1


#设置坐标轴
ax=plt.gca()      
ax.spines["right"].set_color("none")   #设置坐标轴的颜色
ax.spines["top"].set_color("none")
ax.spines["left"].set_position(("data",0))  #设置坐标轴的位置
ax.spines["bottom"].set_position(("data",0))
ax.xaxis.set_ticks_position("bottom")   #x轴的数据显示在x轴右侧
ax.yaxis.set_ticks_position("left")    #y轴的数据显示在y轴左侧
plt.axis([-40,40,-40,40])   #指定绘图范围(x负, x正，y负，y正)
plt.grid()     #显示网格线

X = [0,10,20,30,40]
Y = [0,0,4,3,7]

X = np.array(X)
Y = np.array(Y)
print(X)
print(Y)
plt.plot(0, 0, 'bo')  # 绘制紫红色的圆形的点
plt.plot(X,Y,color='r',linewidth=1.0,linestyle='-')  # 绘制

colors = ['k','g','b','r','m','y']
X1 = X
Y1 = Y
for i in range(0,len(X)):
    #plt.cla()
    #角度计算
    #直线与X轴的夹角，坐标：A(0,0) B(2,4)
    move_point_x = X1[i]
    move_point_y = Y1[i]
    x_value = [0,move_point_x] #x坐标集
    y_value = [0,move_point_y] #y坐标集
    agl=Angle(x_value,y_value)
    print(move_point_x)
    print(move_point_y)
    print(x_value)
    print(y_value)
    print(agl)

    #坐标旋转
    if(agl>0): #顺时针
        Pointx ,Pointy = Srotate(agl,X1,Y1,0,0)
        plt.plot(Pointx,Pointy,color=colors[i],linewidth=1.0,linestyle='--')  # 绘制紫红色的圆形的点
        print(Pointx)
        print(Pointy)
        print(Pointx[i])
        print(Pointy[i])
        X1 = Pointx
        Y1 = Pointy
    else: #逆时针
        Pointx ,Pointy = Nrotate(abs(agl),X1,Y1,0,0)
        plt.plot(Pointx,Pointy,color=colors[i],linewidth=1.0,linestyle='--')  # 绘制紫红色的圆形的点
        print(Pointx)
        print(Pointy)
        print(Pointx[i])
        print(Pointy[i])
        X1 = Pointx
        Y1 = Pointy    

    #坐标移动
    X1=X1-X1[i] #矩阵减法（向量移动）
    print(X1)
    print(Y1)
    plt.plot(X1,Y1,color=colors[i],linewidth=1.0,linestyle='-')  # 绘制

    i = i+1

    plt.pause(2)

plt.show()